#include <buola/chart/cxychart.h>
#include <buola/chart/plots/clineplot.h>
#include <buola/chart/plots/cscatterplot.h>
#include <buola/app/capp.h>
#include <buola/app/ccmdline.h>
#include <buola/algorithm/comparison.h>
#include <buola/zz.h>
#include <buola/zz/zreal.h>
#include <buola/zz/zchar.h>
#include <buola/zz/operators.h>
#include <buola/mat/cquat.h>
#include <robot/kuka/ckinematics.h>

using namespace buola;

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);

    try
    {
        std::vector<CPoint_d> lAllPoints;
        
        
        for(int a=0;a<cmd_line().ArgCount();a++)
        {
            io::CTextReader lS(io::arg(a));
            
            std::string lLine;
            
            auto lRule=+zz::ZReal<double>();
            auto lSkipper=zz::chr(" ");
            
            int i=0;
            while(get_line(lS,lLine))
            {
                if(i++%5!=3) continue;
                std::vector<double> lNums;
                    
                zz::phrase_parse(lLine.begin(),lLine.end(),lRule,lSkipper,lNums);
                
                robot::kuka::CArmPose lPose(lNums);
                
                mat::CVec3d lPos;
                mat::CQuat_d lRot;
                
                robot::forward_kinematics(lPose,lPos,lRot);
                
                lAllPoints.emplace_back(lPos.y(),lPos.z());
            }
        }

        CApp lApp;
        chart::PXYChart lChart=new chart::CXYChart;

        chart::PScatterPlot lPlot=new chart::CScatterPlot;
        lPlot->SetSeries(new chart::CSeries(lAllPoints));
        lPlot->SetName(L"points");
        lPlot->SetColor(img::CColor(L"darkgreen"));
        lPlot->SetPointShape(chart::CPlot::CROSS);
        lPlot->SetPointDiameter(5);
        
        lChart->AddPlot(lPlot);
        lChart->Map(CSize_d(800,600));
        lChart->SetTitle(L"grasp positions");
        
        lApp.Run();
    }
    catch(std::exception &pE)
    {
        msg_info() << pE.what() << "\n";
    }

    return buola_finish();
}